/*
 * Team Name: Focus
 * Team Members: Diana Groun, Edward Bassan
 * File Name; controller.c
 * Description: This file contains the methods that the controller operation needs.
 * Methods such as fetch execute ect.
 */

#include <stdio.h>
#include <stdlib.h>
#include "controller.h"
#include "global.h"

// Constructor

ControllerPtr newController(void) {
	// Initializing all fields
	ControllerPtr controller = (ControllerPtr) malloc(sizeof(ControllerStr));
	controller->cpu = newCpu();
	controller->memoryBlock = newMemory();
	controller->cpu->alu->operandA = newRegister();
	controller->cpu->alu->operandB = newRegister();
	controller->cpu->alu->resultR = newRegister();
	controller->address = newRegister();
	controller->cpu->registers = newRegisterFile();
	controller->modifier = 0;
	controller->opcode = 0;

	//Pointers to functions.
	controller->fetch = fetch;
	controller->decode = decode;
	controller->evaluateAddress = evaluateAddress;
	controller->fetchOperands = fetchOperands;
	controller->execute = execute;
	controller->storeAt = storeAt;
	controller->switchCycle = switchCycle;
	controller->startCycle = startCycle;

	return controller;
}
//Interrupt Vector Table
int IOMAP[] = { 0x2100, 0x2102, 0x2104, 0x2106, 0x2108, 0x2110, 0x2112, 0x2114 };
/*
 * Starts the program cycle so that the program is running
 */
int startCycle(ControllerPtr this) {
	int state = FETCH
	;
	while (state < 6) {
		this->switchCycle(this, &state);
	}
	return NO_ERROR;
}

/*
 * Fetches the next instruction in the program
 */
int fetch(ControllerPtr this, int*state) {

	//printf("\n\tFetching\n");

	// Loading the MAR with the content of PC
	this->cpu->MAR->value = this->cpu->PC->value;

	// Loading the instruction at the address in the MAR into MDR
	this->cpu->MDR->value = this->memoryBlock->fetchWord(this->memoryBlock,
			this->cpu->MAR->value);

	//Loading the IR with the contents of MDR
	this->cpu->IR->value = this->cpu->MDR->value;

	// Increment PC
	this->cpu->PC->value = this->cpu->PC->value + 2;

	return NO_ERROR;
}

/**
 * Decodes the IR and gets the opcode.
 */
int decode(ControllerPtr this, int *state) {
	//printf("\n\tDecoding\n");

	// Getting the opcode from the IR
	this->opcode = ((this->cpu->IR->value & BITS_15_11_MASK) >> 11);
	return NO_ERROR;
}

/**
 * Evaluates the address required for operation.
 */
int evaluateAddress(ControllerPtr this, int *state) {
	//printf("\n\tEvaluating Address.\n");

	// temporary values for easy access
	unsigned short ir = this->cpu->IR->value;

	int operation = this->opcode;
	int mod;


	// Switching between operations depending on the opcode so that the address is
	//evaluated  and evaluating the address according to the instruction
	switch (operation) {
	case LDI:
		this->address->value = Sext(getImm7(ir));
		break;
	case LDA:
		this->address->value = this->cpu->PC->value;
		break;
	case LDB:
		mod = ir & BITS_2_0_MASK;
		switch (mod) {
		case 0x0:
			this->address->value =
					this->cpu->registers->registers[getRa(ir)]->value;
			break;
		case 0x1:
			this->address->value =
					this->cpu->registers->registers[getRa(ir)]->value
							+ this->cpu->registers->registers[0xE]->value;
			break;
		case 0x2:
			this->address->value = this->cpu->registers->registers[12]->value;
			break;
		case 0x3:
			this->address->value = this->memoryBlock->fetchWord(
					this->memoryBlock,
					this->cpu->registers->registers[getRa(ir)]->value);
			break;
		}
		break;
	case LDW:
		mod = ir & BITS_2_0_MASK;
		switch (mod) {
		case 0x0:
			this->address->value =
					this->cpu->registers->registers[getRa(ir)]->value;
			//printf("%X",this->address->value);
			break;
		case 0x1:
			this->address->value =
					this->cpu->registers->registers[getRa(ir)]->value
							+ this->cpu->registers->registers[0xE]->value;
			break;
		case 0x2:
			this->address->value = this->cpu->registers->registers[12]->value;
			break;
		case 0x3:
			this->address->value = this->memoryBlock->fetchWord(
					this->memoryBlock,
					this->cpu->registers->registers[getRa(ir)]->value);
			break;
		}
		break;
	case STB:
		mod = ir & BITS_2_0_MASK;
		switch (mod) {
		case 0x0:
			this->address->value =
					this->cpu->registers->registers[getRa(ir)]->value;
			break;
		case 0x1:
			this->address->value =
					this->cpu->registers->registers[getRa(ir)]->value
							+ this->cpu->registers->registers[0xE]->value;
			//printf("%X", this->address->value);
			break;
		case 0x2:
			this->address->value = this->cpu->registers->registers[0xD]->value;
			printf("%X", this->address->value);
			break;
		case 0x3:
			this->address->value = this->memoryBlock->fetchWord(
					this->memoryBlock,
					this->cpu->registers->registers[getRa(ir)]->value);
			printf("%X", this->address->value);
			break;
		}
		break;
	case STW:
		mod = ir & BITS_2_0_MASK;
		switch (mod) {
		case 0x0:
			this->address->value =
					this->cpu->registers->registers[getRa(ir)]->value;
			break;
		case 0x1:
			this->address->value =
					this->cpu->registers->registers[getRa(ir)]->value
							+ this->cpu->registers->registers[0xE]->value;
			printf("%X", this->address->value);
			break;
		case 0x2:
			this->address->value = this->cpu->registers->registers[0xD]->value;
			printf("%X", this->address->value);
			break;
		case 0x3:
			this->address->value = this->memoryBlock->fetchWord(
					this->memoryBlock,
					this->cpu->registers->registers[getRa(ir)]->value);
			printf("%X", this->address->value);
			break;
		}
		break;
	case POPB:
		mod = ir & BITS_2_0_MASK;
		if ((mod == 0x2) | (mod == 0x4)) {
			this->cpu->registers->registers[SP]->value++;
		}
		this->address->value = this->cpu->registers->registers[SP]->value;
		break;
	case JSR:
		this->address->value = this->cpu->registers->registers[SP]->value;
		break;
	case JSRC:
		this->address->value = this->cpu->registers->registers[SP]->value;
		break;
	case RET:
		this->cpu->registers->registers[SP]->value++;
		this->address->value = this->cpu->registers->registers[SP]->value;
		break;
	case TRAP:
		this->address->value = IVT + Sext(getRv(ir));
		break;
	case IN:
		this->address->value = getPort6(ir);
		break;
	}

	return NO_ERROR;

}

/**
 * Gets the operands.
 */
int fetchOperands(ControllerPtr this) {
	//printf("\n\tFetching Operands\n");

	// temporary fields for easy access.
	unsigned short mod;
	unsigned short ir = this->cpu->IR->value;
	int operation = this->opcode;

	// Switching between operations depending on the opcode and getting the operands
	// according to  the instruction

	switch (operation) {
	case LDI:
		this->cpu->alu->operandA->value = this->address->value;
		break;
	case LDA:
		this->cpu->alu->operandA->value = this->memoryBlock->fetchWord(
				this->memoryBlock, this->address->value);
		break;
	case LDB:
		this->cpu->alu->operandA->value = this->memoryBlock->fetchByte(
				this->memoryBlock, this->address->value);

		break;
	case LDW:
		mod = ir & BITS_2_0_MASK;
		if (mod <= 0x3) {
			this->cpu->alu->operandA->value = this->memoryBlock->fetchWord(
					this->memoryBlock, this->address->value);
		} else {
			this->cpu->alu->operandA->value =
					this->cpu->registers->registers[getRa(ir)]->value;
		}

		break;
	case STB:

		this->cpu->alu->operandA->value = this->cpu->registers->registers[getRd(
				ir)]->value;

		break;
	case STW:

		this->cpu->alu->operandA->value = this->cpu->registers->registers[getRd(
				ir)]->value;
		break;
	case PUSHB:
		mod = ir & BITS_2_0_MASK;
		if (mod == 0x1) {
			this->cpu->alu->operandA->value =
					this->cpu->registers->registers[getRd(ir)]->value;
		} else if (mod == 0x2) {
			this->cpu->alu->operandA->value = this->memoryBlock->fetchByte(
					this->memoryBlock,
					this->cpu->registers->registers[SP]->value);
		} else if (mod == 0x3) {
			this->cpu->alu->operandA->value =
					this->cpu->registers->registers[getRd(ir)]->value;
		} else if (mod == 0x4) {
			this->cpu->alu->operandA->value = this->memoryBlock->fetchWord(
					this->memoryBlock, this->address->value);

		}
		break;
	case ADD:
		mod = ir & BITS_2_0_MASK;
		if ((mod == 0x0) | (mod == 0x1) | (mod == 0x3) | (mod == 0x4)
				| (mod == 0x5) | (mod == 0x6)) {
			this->cpu->alu->operandA->value =
					this->cpu->registers->registers[getRd(ir)]->value;
			this->cpu->alu->operandB->value =
					this->cpu->registers->registers[getRa(ir)]->value;
		} else if (mod == 2) {
			this->cpu->alu->operandA->value =
					this->cpu->registers->registers[getRd(ir)]->getLOB(
							this->cpu->registers->registers[getRd(ir)]);
			this->cpu->alu->operandB->value =
					this->cpu->registers->registers[getRa(ir)]->getLOB(
							this->cpu->registers->registers[getRa(ir)]);
		} else if (mod == 7) {
			this->cpu->alu->operandA->value =
					this->cpu->registers->registers[getRd(ir)]->value;
		}
		break;

	case SHL:
		this->cpu->alu->operandA->value =
				this->cpu->registers->getRegisterValue(this->cpu->registers,
						getRd(ir));
		break;
	case SHR:
		this->cpu->alu->operandA->value =
				this->cpu->registers->getRegisterValue(this->cpu->registers,
						getRd(ir));
		break;
	case BRI:
		this->cpu->alu->operandA->value = Sext(getImm11(ir));
		this->cpu->alu->operandB->value = this->cpu->PC->value;
		break;
	case BR:
		this->cpu->alu->operandA->value = this->cpu->registers->registers[getRd(
				ir)]->value-2;
		printf("\n\t%X",this->cpu->alu->operandA->value);
		break;
	case BRIC:
		this->cpu->alu->operandA->value = Sext(getImm9(ir));
		this->cpu->alu->operandB->value = this->cpu->PC->value;
		break;
	case BRC:
		this->cpu->alu->operandB->value = this->cpu->PC->value;
		this->cpu->alu->operandA->value = this->cpu->registers->registers[getRd(
				ir)]->value- this->cpu->PC->value-2;
		break;
	case JSR:
		this->cpu->alu->operandA->value = this->cpu->registers->registers[getRd(
				ir)]->value;
		break;
	case JSRC:
		this->cpu->alu->operandA->value = this->cpu->registers->registers[getRd(
				ir)]->value;
		printf("%X",this->cpu->alu->operandA->value);
		break;
	case RET:
		this->cpu->alu->operandA->value = this->memoryBlock->fetchWord(
				this->memoryBlock, this->address->value);
		break;
	case TRAP:
		this->cpu->alu->operandA->value = this->address->value;
		break;
	case IN:
		mod = ir & BITS_1_0_MASK;
		if(mod == 0 ) {
			this->cpu->alu->operandA->value = io(this);
		} else {
			this->cpu->alu->operandA->value = this->cpu->registers->registers[getRd(ir)]->value;
			io(this);
		}
		break;
	}
	return NO_ERROR;
}

unsigned short io(ControllerPtr this){
	unsigned short Port6 = this->address->value;
	if(Port6 == 0x3A){
		//printf("Getting input");
		char i;
		printf("\nDisplay\n");
		printf("-----------------------------------\n");
		scanf("%s",&i);
		printf("-----------------------------------\n");
		return i;
	}else if(Port6 == 0x32){
		printf("\nDisplay\n");
		printf("-----------------------------------\n");
		printf("%c\n",this->cpu->alu->operandA->value);
		printf("-----------------------------------");
	}
	return NO_ERROR;
}
/**
 * Execute the operation.
 */
int execute(ControllerPtr this, int *state) {
	printf("\n");
	printf("%X\t",this->cpu->MAR->value);
	//printf("\n\tExecuting\n");
	int mod;
	unsigned short ir = this->cpu->IR->value;
	int operation = this->opcode;

	//Executing the actual instruction and storing in the result register
	switch (operation) {
	case LDI:
		printf("LDI $R%X, #%d", getRd(ir), getImm7(ir));
		this->cpu->alu->resultR->value = this->cpu->alu->operandA->value;
		break;
	case LDA:
		printf("LDA $R%X, %.4X", getRd(ir), this->address->value);
		this->cpu->alu->resultR->value = this->cpu->alu->operandA->value;
		this->cpu->PC->value = this->cpu->PC->value + 2;
		break;
	case LDB:
		mod = ir & BITS_2_0_MASK;
		this->cpu->alu->resultR->value = this->cpu->alu->operandA->value;
		switch (mod) {
		case 0x0:
			printf("LDB $R%X, $R%X", getRd(ir), getRa(ir));
			break;
		case 0x1:
			printf("LDB $R%X, %%$R%X", getRd(ir), getRa(ir));
			break;
		case 0x2:
			printf("LDB &$R%X", getRd(ir));
			this->cpu->registers->registers[0xC]->value++;
			break;
		case 0x3:
			printf("LDB $R%X, @$R%X", getRd(ir), getRa(ir));
			break;
		}
		break;

	case LDW:
		mod = ir & BITS_2_0_MASK;
		this->cpu->alu->resultR->value = this->cpu->alu->operandA->value;
		switch (mod) {
		case 0x0:
			printf("LDW $R%X, $R%X", getRd(ir), getRa(ir));
			break;
		case 0x1:
			printf("LDW $R%X, %%$R%X", getRd(ir), getRa(ir));
			break;
		case 0x2:
			printf("LDW &$R%X", getRd(ir));
			this->cpu->registers->registers[0xC]->value++;
			this->cpu->registers->registers[0xC]->value++;
			break;
		case 0x3:
			printf("LDW $R%X, @$R%X", getRd(ir), getRa(ir));
			break;
		case 0x4:
			printf("MOV $R%X, $R%X", getRd(ir), getRa(ir));
			break;
		}
		break;
		this->cpu->alu->resultR->value = this->cpu->alu->operandA->value;
		break;
	case STB:
		mod = ir & BITS_2_0_MASK;
		this->cpu->alu->resultR->value = this->cpu->alu->operandA->value;
		switch (mod) {
		case 0x0:
			printf("STB $R%d, $R%d", getRd(ir), getRa(ir));
			break;
		case 0x1:
			printf("STB $R%d, %%$R%d", getRd(ir), getRa(ir));
			break;
		case 0x2:
			printf("STB &$R%d", getRd(ir));
			this->cpu->registers->registers[0xD]->value++;
			break;
		case 0x3:
			printf("STB $R%d, @$R%d", getRd(ir), getRa(ir));
			break;
		}
		break;
	case STW:
		mod = ir & BITS_2_0_MASK;
		this->cpu->alu->resultR->value = this->cpu->alu->operandA->value;
		switch (mod) {
		case 0x0:
			printf("STW $R%d, $R%d", getRd(ir), getRa(ir));
			break;
		case 0x1:
			printf("STW $R%d, %%$R%d", getRd(ir), getRa(ir));
			break;
		case 0x2:
			printf("STW &$R%d", getRd(ir));
			this->cpu->registers->registers[0xD]->value++;
			this->cpu->registers->registers[0xD]->value++;
			break;
		case 0x3:
			printf("STW $R%d, @$R%d", getRd(ir), getRa(ir));
			break;
		}
		break;
	case PUSHB:
		mod = ir & BITS_2_0_MASK;
		switch (mod) {
		case 0x1:
			printf("PUSHB $R%d", getRd(ir));
			this->memoryBlock->storeByte(this->memoryBlock,
					this->address->value, this->cpu->alu->operandA->value);
			this->cpu->registers->registers[SP]->value--;
			break;
		case 0x2:
			this->cpu->registers->registers[getRd(ir)]->value =
					this->cpu->alu->operandA->value;
			this->cpu->registers->registers[SP]->value++;
			printf("POPB $R%d", getRd(ir));
			break;
		case 0x3:
			this->memoryBlock->storeWord(this->memoryBlock,
					this->address->value, this->cpu->alu->operandA->value);
			this->cpu->registers->registers[SP]->value--;
			this->cpu->registers->registers[SP]->value--;
			printf("PUSHW $R%d", getRd(ir));
			break;
		case 0x4:
			this->cpu->registers->registers[getRd(ir)]->value =
					this->cpu->alu->operandA->value;
			this->cpu->registers->registers[SP]->value++;
			printf("POP $R%d", getRd(ir));
			break;
		}
		break;
	case ADD:
		mod = ir & BITS_2_0_MASK;
		switch (mod) {
		case 0x0:
			this->cpu->alu->addition(this->cpu->alu);
			printf("ADD $R%X, $R%X", getRd(ir), getRa(ir));
			break;
		case 0x1:
			this->cpu->alu->subtraction(this->cpu->alu);
			printf("SUB $R%X, $R%X", getRd(ir), getRa(ir));
			break;
		case 0x2:
			this->cpu->alu->multiplication(this->cpu->alu);
			printf("MUL $R%X, $R%X", getRd(ir), getRa(ir));
			break;
		case 0x3:
			this->cpu->alu->division(this->cpu->alu,
					this->cpu->registers->registers[8]);
			printf("DIV $R%X, $R%X", getRd(ir), getRa(ir));
			break;
		case 0x4:
			this->cpu->alu->and(this->cpu->alu);
			printf("AND $R%X, $R%X", getRd(ir), getRa(ir));
			break;
		case 0x5:
			this->cpu->alu->or(this->cpu->alu);
			printf("OR $R%X, $R%X", getRd(ir), getRa(ir));
			break;
		case 0x6:
			this->cpu->alu->xor(this->cpu->alu);
			printf("XOR $R%X, $R%X", getRd(ir), getRa(ir));
			break;
		case 0x7:
			this->cpu->alu->not(this->cpu->alu);
			printf("NOT $R%X", getRd(ir));
			break;
		}
		break;
	case SHL:
		printf("SHL $R%X", getRd(ir));
		this->cpu->alu->shiftLeft(this->cpu->alu);
		break;
	case SHR:
		printf("SHR $R%X", getRd(ir));
		this->cpu->alu->shiftRight(this->cpu->alu);
		break;
	case BRI:
		printf("BR #%d", getImm11(ir));
		this->cpu->alu->addition(this->cpu->alu);
		break;
	case BR:
		printf("BR $R%X", getRd(ir));
		this->cpu->alu->resultR->value = this->cpu->alu->operandA->value;
		break;
	case BRIC:
		mod = ir & BITS_1_0_MASK;
		if (mod == 0x0) {
			this->cpu->alu->resultR->value = this->cpu->alu->operandB->value;
			printf("BRn #%d", getImm9(ir));
			if (this->cpu->alu->negFlag == 0x1) {
				this->cpu->alu->addition(this->cpu->alu);
			}
		} else if (mod == 0x1) {
			this->cpu->alu->resultR->value = this->cpu->alu->operandB->value;
			printf("BRz #%d", getImm9(ir));
			if (this->cpu->alu->zeroFlag == 0x1) {
				this->cpu->alu->addition(this->cpu->alu);
			}
		} else if (mod == 0x2) {
			this->cpu->alu->resultR->value = this->cpu->alu->operandB->value;
			printf("BRc #%d", getImm9(ir));
			if (this->cpu->alu->carryFlag == 0x1) {
				this->cpu->alu->addition(this->cpu->alu);
			}
		} else if (mod == 0x3) {
			this->cpu->alu->resultR->value = this->cpu->alu->operandB->value;
			printf("BRo #%d", getImm9(ir));
			if (this->cpu->alu->overflowFlag == 0x1) {
				this->cpu->alu->addition(this->cpu->alu);
			}
		}

		break;
	case BRC:
		mod = ir & BITS_1_0_MASK;
		if (mod == 0x0) {
			this->cpu->alu->resultR->value = this->cpu->alu->operandB->value;
			printf("BRn $R%X", getRd(ir));
			if (this->cpu->alu->negFlag == 0x1) {
				this->cpu->alu->addition(this->cpu->alu);
			}
		} else if (mod == 0x1) {
			this->cpu->alu->resultR->value = this->cpu->alu->operandB->value;
			printf("BRz $R%X", getRd(ir));
			if (this->cpu->alu->zeroFlag == 0x1) {
				this->cpu->alu->addition(this->cpu->alu);
			}
		} else if (mod == 0x2) {
			this->cpu->alu->resultR->value = this->cpu->alu->operandB->value;
			printf("BRc $R%X", getRd(ir));
			if (this->cpu->alu->carryFlag == 0x1) {
				this->cpu->alu->addition(this->cpu->alu);
			}
		} else if (mod == 0x3) {
			this->cpu->alu->resultR->value = this->cpu->alu->operandB->value;
			printf("BRo $R%X", getRd(ir));
			if (this->cpu->alu->overflowFlag == 0x1) {
				this->cpu->alu->addition(this->cpu->alu);
			}
		}

		break;
	case JSR:
		this->cpu->alu->resultR->value = this->cpu->alu->operandA->value;
		this->memoryBlock->storeWord(this->memoryBlock, this->address->value,
				this->cpu->PC->value);
		this->cpu->registers->registers[SP]->value--;
		this->cpu->registers->registers[SP]->value--;

		printf("JSR $R%d", getRd(ir));
		break;
	case JSRC:
		mod = ir & BITS_1_0_MASK;
		if (mod == 0x0) {
			printf("JSRn $R%X", getRd(ir));
			if (this->cpu->alu->negFlag == 0x1) {
				this->cpu->alu->resultR->value = this->cpu->alu->operandA->value;
				this->memoryBlock->storeWord(this->memoryBlock, this->address->value,
						this->cpu->PC->value);
				this->cpu->registers->registers[SP]->value--;
				this->cpu->registers->registers[SP]->value--;
			}
		} else if (mod == 0x1) {
			printf("JSRz $R%X", getRd(ir));
			if (this->cpu->alu->zeroFlag == 0x1) {
				this->cpu->alu->resultR->value = this->cpu->alu->operandA->value;
				this->memoryBlock->storeWord(this->memoryBlock, this->address->value,
						this->cpu->PC->value);
				this->cpu->registers->registers[SP]->value--;
				this->cpu->registers->registers[SP]->value--;
			}
		} else if (mod == 0x2) {
			printf("JSRc $R%X", getRd(ir));
			if (this->cpu->alu->carryFlag == 0x1) {
				this->cpu->alu->resultR->value = this->cpu->alu->operandA->value;
				this->memoryBlock->storeWord(this->memoryBlock, this->address->value,
						this->cpu->PC->value);
				this->cpu->registers->registers[SP]->value--;
				this->cpu->registers->registers[SP]->value--;
			}
		} else if (mod == 0x3) {
			printf("JSRo $R%X", getRd(ir));
			if (this->cpu->alu->overflowFlag == 0x1) {
				this->cpu->alu->resultR->value = this->cpu->alu->operandA->value;
				this->memoryBlock->storeWord(this->memoryBlock, this->address->value,
						this->cpu->PC->value);
				this->cpu->registers->registers[SP]->value--;
				this->cpu->registers->registers[SP]->value--;
			}
		}
		break;
	case RET:
		printf("%X",this->cpu->alu->operandA->value);
		this->cpu->alu->resultR->value = this->cpu->alu->operandA->value;
		this->cpu->registers->registers[SP]->value++;
		printf("RET");
		break;
	case TRAP:
		printf("%X",this->cpu->alu->operandA->value);
		this->cpu->alu->resultR->value = this->cpu->alu->operandA->value;
		break;
	case IN:
		mod = ir & BIT_0_MASK;
		if(mod == 0) {
			printf("IN $R%X #%X",getRd(ir),getPort6(ir));
			this->cpu->alu->resultR->value = this->cpu->alu->operandA->value;
		} else {
			printf("OUT $R%X #%X",getRd(ir),getPort6(ir));
			this->cpu->alu->resultR->value = this->cpu->alu->operandA->value;
		}
		break;
	}
	return NO_ERROR;
}
/**
 * Stores the result to the destination register.
 */
int storeAt(ControllerPtr this, int *state) {
	//printf("\n\tStoring\n");
	int mod;
	unsigned short ir = this->cpu->IR->value;
	switch (this->opcode) {
	case LDI:
		this->cpu->registers->registers[getRd(this->cpu->IR->value)]->value =
				this->cpu->alu->resultR->value;
		break;
	case LDA:
		this->cpu->registers->registers[getRd(this->cpu->IR->value)]->value =
				this->cpu->alu->resultR->value;
		break;
	case LDB:
		this->cpu->registers->registers[getRd(this->cpu->IR->value)]->value =
				this->cpu->alu->resultR->value;
		break;
	case LDW:
		this->cpu->registers->registers[getRd(this->cpu->IR->value)]->value =
				this->cpu->alu->resultR->value;
		break;
	case STB:
		this->memoryBlock->storeByte(this->memoryBlock, this->address->value,
				this->cpu->alu->resultR->value);
		break;
	case STW:
		this->memoryBlock->storeWord(this->memoryBlock, this->address->value,
				this->cpu->alu->resultR->value);
		break;
	case ADD:
		this->cpu->registers->registers[0]->value =
				this->cpu->alu->resultR->value;
		break;

	case SHL:
		this->cpu->registers->setRegister(this->cpu->registers, 0,
				this->cpu->alu->resultR->value);
		break;
	case SHR:
		this->cpu->registers->setRegister(this->cpu->registers, 0,
				this->cpu->alu->resultR->value);
		break;
	case BRI:
		this->cpu->PC->value = this->cpu->alu->resultR->value;
		break;
	case BR:
		this->cpu->PC->value = this->cpu->alu->resultR->value;
		break;
	case BRIC:
		this->cpu->PC->value = this->cpu->alu->resultR->value;
		break;
	case BRC:
		this->cpu->PC->value = this->cpu->alu->resultR->value;
		break;
	case JSR:
		this->cpu->PC->value = this->cpu->alu->resultR->value;
		break;
	case JSRC:
		this->cpu->PC->value = this->cpu->alu->resultR->value;
		break;
	case RET:
		this->cpu->PC->value = this->cpu->alu->resultR->value;
		break;
	case TRAP:
		this->cpu->PC->value = this->cpu->alu->resultR->value;
		break;
	case IN:
	    mod = ir & BIT_0_MASK;
		if(mod == 0) {
		this->cpu->registers->registers[getRd(this->cpu->IR->value)]->value =
						this->cpu->alu->resultR->value;
		} else {
			IOMAP[getPort6(ir)] = this->cpu->alu->resultR->value;
		}
		break;
	case NOP:
		printf("No Program to run\n");
		*state = EXIT
		this->cpu->PC->value = 0x3000;
		;
		break;
	case HALT:
		printf("HALT\n");
		*state = EXIT
		this->cpu->PC->value = 0x3000;
		;
		break;
	}
	return NO_ERROR;
}

/**
 * The Clock Cycle of the SC2.
 */
int switchCycle(ControllerPtr this, int *state) {
	switch (*state) {
	case 0:
		*state = DECODE
		;
		fetch(this, &(*state));
		break;

	case 1:
		*state = EVALUATE
		;
		;
		decode(this, &(*state));
		break;

	case 2:
		*state = FETCH_OPERANDS
		;
		;
		evaluateAddress(this, &(*state));
		break;

	case 3:
		*state = EXECUTE
		;
		fetchOperands(this);
		break;

	case 4:
		*state = STORE
		;
		execute(this, &(*state));
		break;

	case 5:
		*state = FETCH
		;
		storeAt(this, state);
		break;

	case 6:
		this->cpu->registers->setRegister(this->cpu->registers, 11, 0);
		break;
	}
	return NO_ERROR;
}

/*
 * Gets the immediate 7 value form the IR
 */
unsigned short getImm7(unsigned short instructionReg) {
	return ((instructionReg & BITS_6_0_MASK));
}

/*
 * Gets the immediate 9 value from the IR
 */
unsigned short getImm9(unsigned short instructionReg) {
	return ((instructionReg & BITS_A_2_MASK)) >> 2;
}

/*
 * Gets the immediate 11 value from the IR
 */
unsigned short getImm11(unsigned short instructionReg) {
	return ((instructionReg & BITS_10_0_MASK));
}

/**
 * Sign extends the given value to 16 bits
 */
unsigned short Sext(unsigned short value) {
	signed short sext_value = value;
	return sext_value;
}
/*
 * Gets the vector Register number from the IR
 */
int getRv(unsigned short instructionReg) {
	return ((instructionReg & BITS_A_5_MASK) >> 5);
}
/*
 * Gets the Register Port number from the IR
 */
int getPort6(unsigned short instructionReg) {
	return ((instructionReg & BITS_6_1_MASK) >> 1);
}
/*
 * Gets the second register number from the IR
 */
int getRs(unsigned short instructionReg) {
	return ((instructionReg & BITS_A_7_MASK) >> 7);
}
/*
 * Gets the 1st Register number from the IR
 */
int getRd(unsigned short instructionReg) {
	return ((instructionReg & BITS_A_7_MASK) >> 7);
}

/*
 * Gets the second register number from the IR
 */
int getRa(unsigned short instruction) {
	return ((instruction & BITS_6_3_MASK) >> 3);
}

/**
 * Test for the controller.c
 */
/*
int main(void)
{

	int state = 0;
	ControllerPtr test = newController();
	test->fetch(test,&state);
	test->decode(test, &state);
	test->evaluateAddress(test, &state);
	test->fetchOperands(test);
	test->execute(test, &state);
	test->storeAt(test,&state);

	return 0;
}
*/
